#include <ArduinoBLE.h>

static constexpr uint8_t SENSOR_INFO_TEMP_OFFSET = 19;
static constexpr uint8_t SENSOR_INFO_HUM_OFFSET = 21;
static constexpr uint8_t SENSOR_INFO_ACCELERATION_OFFSET = 13;


inline int16_t convertToInt16(uint8_t* data, uint8_t offset){
    return (int16_t)((uint16_t)data[offset] << 8 | data[offset + 1]);
}
inline uint16_t convertToUInt16(uint8_t* data, uint8_t offset){
    return (uint16_t)((uint16_t)data[offset] << 8 | data[offset + 1]);
}

void blinkLED(int pin, int times = 5, int delayTime = 200) {
    for (int i = 0; i < times; i++) {
        digitalWrite(pin, !digitalRead(pin));
        delay(delayTime);
        digitalWrite(pin, !digitalRead(pin));
        delay(delayTime);
    }
}


String frameToString(uint8_t* adv_data, uint8_t data_length) {
    // Returns a string representation of the frame type as a list of 0xXX hex values
    String result;
    for (int i = 0; i < data_length; i++) {
        result += String(adv_data[i], HEX);
        if (i < data_length - 1) {
            result += " ";
        }
    }
    return result;
}


void setup() {
    Serial.begin(115200);
    pinMode(LEDR, OUTPUT);
    pinMode(LED_BUILTIN, OUTPUT);
    pinMode(LED_USER, OUTPUT);
    digitalWrite(LED_BUILTIN, LOW);
    digitalWrite(LEDR, LOW);
    digitalWrite(LED_USER, LOW);
    
    // Wait 5 seconds for Serial connection before continuing
    for(unsigned long const serialBeginTime = millis(); !Serial && (millis() - serialBeginTime <= 5000););

    if (!BLE.begin()) {
        Serial.println("Starting Bluetooth® Low Energy module failed!");
        while (1);
    }

    digitalWrite(LED_BUILTIN, HIGH);
    Serial.println("Starting BLE scan...");
    BLE.scan(true);
}

void loop() {
    BLEDevice device = BLE.available();
    if (!device) return;
    
    int len = device.advertisementDataLength();

    if(len < 8){
        // Doesn't seem to be a valid frame type, can't check anyway with just 8 bytes
        return;
    }

    uint8_t data[62];
    device.advertisementData(data, len);    

    // Check for service UUID
    if(!(data[5] == 0x01 && data[6] == 0xEA)) return;

    // Check for specific frame
    if(data[7] != 0x80) return;

    if(len > 62){
        digitalWrite(LED_USER, HIGH);
        Serial.println(String("Invalid advertisement data length: ") + String(len));
        return;
    }

    // Make sure the data is long enough to contain the field with the highest offset
    if(len < SENSOR_INFO_HUM_OFFSET + 2){
        digitalWrite(LED_USER, HIGH);
        Serial.println(String("Advertisement data length too short: ") + String(len));
        return;
    }

    int16_t temperatureRaw = convertToInt16(data, SENSOR_INFO_TEMP_OFFSET);
    uint16_t humidityRaw = convertToUInt16(data, SENSOR_INFO_HUM_OFFSET);
    int16_t accelerationXRaw = convertToInt16(data, SENSOR_INFO_ACCELERATION_OFFSET);
    int16_t accelerationYRaw = convertToInt16(data, SENSOR_INFO_ACCELERATION_OFFSET + 2);
    int16_t accelerationZRaw = convertToInt16(data, SENSOR_INFO_ACCELERATION_OFFSET + 4);

    float temperature = temperatureRaw / 10.0;
    float humidity = humidityRaw / 10.0;
    float accelerationX = accelerationXRaw / 1000.0;
    float accelerationY = accelerationYRaw / 1000.0;
    float accelerationZ = accelerationZRaw / 1000.0;

    // Serial.print(device.address());
    // if (device.hasLocalName()) {
    //     Serial.print(" ");
    //     Serial.print(device.localName());
    // }
    // Serial.println();
    // Serial.print("T: ");
    // Serial.print(temperature);
    // Serial.print(" H: ");
    // Serial.println(humidity);
    // Serial.print("X: ");
    // Serial.print(accelerationX);
    // Serial.print(" Y: ");
    // Serial.print(accelerationY);
    // Serial.print(" Z: ");
    // Serial.println(accelerationZ);

    if(temperature < -20 || temperature > 85){
        digitalWrite(LED_USER, HIGH);
        Serial.println(String("Invalid temperature value: ") + String(temperature));
        Serial.println(String("Frame data: ") + frameToString(data, len));
    }

    if(humidity < 0 || humidity > 100){
        digitalWrite(LED_USER, HIGH);
        Serial.println(String("Invalid humidity value: ") + String(humidity));
        Serial.println(String("Frame data: ") + frameToString(data, len));
    }

    if(accelerationX < -2 || accelerationX > 2){
        digitalWrite(LED_USER, HIGH);
        Serial.println(String("Invalid X acceleration value: ") + String(accelerationX));
        Serial.println(String("Frame data: ") + frameToString(data, len));
    }
    if(accelerationY < -2 || accelerationY > 2){
        digitalWrite(LED_USER, HIGH);
        Serial.println(String("Invalid Y acceleration value: ") + String(accelerationY));
        Serial.println(String("Frame data: ") + frameToString(data, len));
    }
    if(accelerationZ < -2 || accelerationZ > 2){
        digitalWrite(LED_USER, HIGH);
        Serial.println(String("Invalid Z acceleration value: ") + String(accelerationZ));
        Serial.println(String("Frame data: ") + frameToString(data, len));
    }
    // Serial.println("---");
}

#include <ArduinoBLE.h>

#if defined(ARDUINO_ARDUINO_NANO33BLE)
    #include <Arduino_HTS221.h>
    #include <Arduino_LSM9DS1.h>
#elif defined(ARDUINO_NICLA)
    #include "Arduino_BHY2.h"
    SensorXYZ accelerometer(SENSOR_ID_ACC);
    Sensor temperatureSensor(SENSOR_ID_TEMP);
    Sensor humiditySensor(SENSOR_ID_HUM); 
#endif

#define SENSOR_INFO_TEMP_OFFSET 19
#define SENSOR_INFO_HUM_OFFSET 21
#define SENSOR_INFO_ACCELERATION_OFFSET 13

// Advertising parameters should have a global scope. Do NOT define them in 'setup' or in 'loop'
uint8_t completeRawAdvertisingData[] = {
    0x02, // Length of this AD structure (2 bytes follow)
    0x01, // AD type = Flags
    0x06, // Flags = LE General Discoverable Mode | BR/EDR Not Supported
    0x1B, // Length of this AD structure (27 bytes follow)
    0x16, // AD type = Service UUID
    0x01, 0xEA, // Service UUID = 0x0101EA
    0x80, // Frame type UUID: 0x80
    0b10111100, // Sensor status
    0x0, // MAGNET_CNT_LSB
    0x0, // MAGNET_CNT_MSB
    0x0, // MOTION_CNT_LSB
    0x0, // MOTION_CNT_MSB
    0x0, // Accel.Dat_X axis (mg) LSB
    0x0, // Accel.Dat_X axis (mg) MSB
    0x0, // Accel.Dat_Y axis (mg) LSB
    0x0, // Accel.Dat_Y axis (mg) MSB
    0x0, // Accel.Dat_Z axis (mg) LSB
    0x0, // Accel.Dat_Z axis (mg) MSB
    0x0, // Temperature.Dat (C) LSB
    0x0, // Temperature.Dat (C) MSB
    0x0, // Humidity.Dat (%RH) LSB
    0x0, // Humidity.Dat (%RH) MSB
    0x0, // Battery percentage(1%) / Battery voltage (1mV)_LSB
    0x64, // Battery percentage(1%) / Battery voltage (1mV)_MSB
    0x0, // Tag ID[0]
    0x0, // Tag ID[1]
    0x0, // Tag ID[2]
    0x0, // Tag ID[3]
    0x0, // Tag ID[4]
    0x0  // Tag ID[5]
};

BLEAdvertisingData advData;

void updateTemperatureHumidity() {
  #if defined(ARDUINO_ARDUINO_NANO33BLE)
    float temperature = HTS.readTemperature();
    float humidity = HTS.readHumidity();
  #elif defined(ARDUINO_NICLA)
    float temperature = temperatureSensor.value();
    float humidity = humiditySensor.value();
    if(!temperatureSensor.dataAvailable() || !humiditySensor.dataAvailable()) {
        Serial.println("Temperature or humidity sensor not available");
        return;
    }
  #else
    // Generate random values for other boards (range 20-25 °C and 20-80 %RH)
    float temperature = random(200, 250) / 10.0;
    float humidity = random(200, 800) / 10.0;
  #endif

  Serial.print("Temperature: ");
  Serial.print(temperature);
  Serial.print(" °C, Humidity: ");
  Serial.print(humidity);
  Serial.println(" %");

  uint16_t convertedTemperature = (uint16_t)(temperature * 10); // Convert to tenths of degree
  uint16_t convertedHumidity = (uint16_t)(humidity * 10); // Convert to tenths of percent

  completeRawAdvertisingData[SENSOR_INFO_TEMP_OFFSET] = highByte(convertedTemperature);
  completeRawAdvertisingData[SENSOR_INFO_TEMP_OFFSET + 1] = lowByte(convertedTemperature);
  completeRawAdvertisingData[SENSOR_INFO_HUM_OFFSET] = highByte(convertedHumidity);
  completeRawAdvertisingData[SENSOR_INFO_HUM_OFFSET + 1] = lowByte(convertedHumidity);
}

void updateIMU(){
    #if defined(ARDUINO_ARDUINO_NANO33BLE)
    float x, y, z;
    if (IMU.accelerationAvailable()){
        IMU.readAcceleration(x, y, z);
    }
    int16_t accelX = (int16_t)(x * 1000); // Convert to milligravity
    int16_t accelY = (int16_t)(y * 1000); // Convert to milligravity
    int16_t accelZ = (int16_t)(z * 1000); // Convert to milligravity
    #elif defined(ARDUINO_NICLA)
    if(!accelerometer.dataAvailable()) {
        Serial.println("Accelerometer not available");
        return;
    }
    int16_t accelX = accelerometer.x();
    int16_t accelY = accelerometer.y();
    int16_t accelZ = accelerometer.z();
    #else
    // Generate random values for other boards (range -2000 to 2000 mg)
    int16_t accelX = random(-2000, 2000);
    int16_t accelY = random(-2000, 2000);
    int16_t accelZ = random(-2000, 2000);
    #endif

    Serial.print("Acceleration X: ");
    Serial.print(accelX / 1000.0); // Convert to g
    Serial.print(" Y: ");
    Serial.print(accelY / 1000.0); // Convert to g
    Serial.print(" Z: ");
    Serial.println(accelZ / 1000.0); // Convert to g
    
    completeRawAdvertisingData[SENSOR_INFO_ACCELERATION_OFFSET] = highByte(accelX);
    completeRawAdvertisingData[SENSOR_INFO_ACCELERATION_OFFSET + 1] = lowByte(accelX);
    completeRawAdvertisingData[SENSOR_INFO_ACCELERATION_OFFSET + 2] = highByte(accelY);
    completeRawAdvertisingData[SENSOR_INFO_ACCELERATION_OFFSET + 3] = lowByte(accelY);
    completeRawAdvertisingData[SENSOR_INFO_ACCELERATION_OFFSET + 4] = highByte(accelZ);
    completeRawAdvertisingData[SENSOR_INFO_ACCELERATION_OFFSET + 5] = lowByte(accelZ);
}

void updateMACAddress() {
    String address = BLE.address();
    uint8_t addr[6];
    int idx = 0;
    for (int i = 0; i < address.length(); i += 3) {
        String byteStr = address.substring(i, i + 2);
        addr[idx++] = (uint8_t)std::strtol(byteStr.c_str(), nullptr, 16);
    }
    // Update the Tag ID in the advertising data with the MAC address
    for (int i = 0; i < 6; i++) {
        completeRawAdvertisingData[25 + i] = addr[i];
    }
}

void updateSensorData() {
    updateTemperatureHumidity();
    updateIMU();
}

void startSensors(){
  #if defined(ARDUINO_ARDUINO_NANO33BLE)
    HTS.begin();
    IMU.begin();
  #elif defined(ARDUINO_NICLA)
    BHY2.begin();
    temperatureSensor.begin();
    humiditySensor.begin();
    accelerometer.begin();
  #endif
}

void setup() {
  startSensors();
  
  Serial.begin(9600);
  // Wait for 5 seconds to allow the serial monitor to connect
  for(unsigned long const serialBeginTime = millis(); !Serial && (millis() - serialBeginTime <= 5000););

  // Blink to indicate the start of the program
  for (int i = 0; i < 5; i++) {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(100);
    digitalWrite(LED_BUILTIN, LOW);
    delay(100);
  }

  if (!BLE.begin()) {
    Serial.println("failed to initialize BLE!");
    while (1);
  }

  updateMACAddress(); // Update MAC address from the board's BLE address
  updateSensorData(); // Initial sensor data update
  advData.setRawData(completeRawAdvertisingData, sizeof(completeRawAdvertisingData));  
  BLE.setAdvertisingData(advData);
  BLEAdvertisingData scanData; // Build scan response data packet
  scanData.setLocalName("MK Simulator");
  BLE.setScanResponseData(scanData); // Copy set parameters in the actual scan response packet
  BLE.setAdvertisingInterval(1000); // ms, default is 100ms

  BLE.advertise();
  Serial.println("Advertising at " + String(BLE.address()));
}

void loop() {
  #if defined(ARDUINO_NICLA)
    BHY2.update();
  #endif
  BLE.poll();

  static unsigned long lastUpdate = 0;
  unsigned long currentMillis = millis();
    if (currentMillis - lastUpdate >= 1000) { // Update every second
        lastUpdate = currentMillis;
        BLE.stopAdvertise(); // Stop advertising to update data
        updateSensorData(); // Update sensor data
        BLE.advertise();
    }
}